// 定义BTS7960控制引脚（电驱1）
const int RPWM1 = 12;  // 右侧PWM控制引脚
const int LPWM1 = 13;  // 左侧PWM控制引脚

// 定义BTS7960控制引脚（电驱2）
const int RPWM2 = 14;  // 右侧PWM控制引脚
const int LPWM2 = 27;  // 左侧PWM控制引脚

// 定义PWM参数
const int freq = 5000;      // PWM频率为5kHz
const int pwmChannelR1 = 4; // 电驱1右侧PWM通道
const int pwmChannelL1 = 5; // 电驱1左侧PWM通道
const int pwmChannelR2 = 6; // 电驱2右侧PWM通道
const int pwmChannelL2 = 7; // 电驱2左侧PWM通道
const int resolution = 8;   // 8位分辨率，范围0-255

void setup() {
  // 初始化串口通信
  Serial.begin(115200);
  Serial.println("Dual Motor Control Ready!");
  
  // 设置引脚模式
  pinMode(RPWM1, OUTPUT);
  pinMode(LPWM1, OUTPUT);
  pinMode(RPWM2, OUTPUT);
  pinMode(LPWM2, OUTPUT);
  
  // 配置PWM通道
  Serial.print("Configuring PWM Channel R1 (");
  Serial.print(pwmChannelR1);
  Serial.print(") on pin ");
  Serial.println(RPWM1);
  ledcSetup(pwmChannelR1, freq, resolution);
  
  Serial.print("Configuring PWM Channel L1 (");
  Serial.print(pwmChannelL1);
  Serial.print(") on pin ");
  Serial.println(LPWM1);
  ledcSetup(pwmChannelL1, freq, resolution);
  
  Serial.print("Configuring PWM Channel R2 (");
  Serial.print(pwmChannelR2);
  Serial.print(") on pin ");
  Serial.println(RPWM2);
  ledcSetup(pwmChannelR2, freq, resolution);
  
  Serial.print("Configuring PWM Channel L2 (");
  Serial.print(pwmChannelL2);
  Serial.print(") on pin ");
  Serial.println(LPWM2);
  ledcSetup(pwmChannelL2, freq, resolution);
  
  // 将PWM通道绑定到对应引脚
  ledcAttachPin(RPWM1, pwmChannelR1);
  ledcAttachPin(LPWM1, pwmChannelL1);
  ledcAttachPin(RPWM2, pwmChannelR2);
  ledcAttachPin(LPWM2, pwmChannelL2);
  
  // 初始状态下电机停止
  ledcWrite(pwmChannelR1, 0);
  ledcWrite(pwmChannelL1, 0);
  ledcWrite(pwmChannelR2, 0);
  ledcWrite(pwmChannelL2, 0);
  
  Serial.println("System initialized successfully!");
}

void loop() {
  // 检查串口是否有数据可读
  if (Serial.available() > 0) {
    // 读取并解析两个电机的速度值
    int speed1 = 0, speed2 = 0;
    bool hasSpeed1 = false, hasSpeed2 = false;
    
    // 读取完整的一行数据
    if (Serial.available()) {
      String input = Serial.readStringUntil('\n');
      input.trim();
      
      Serial.print("Received input: ");
      Serial.println(input);
      
      // 解析格式："speed1;speed2"
      int semicolonIndex = input.indexOf(';');
      if (semicolonIndex > 0) {
        // 提取第一个数字（电机1速度）
        String speed1Str = input.substring(0, semicolonIndex);
        speed1 = speed1Str.toInt();
        hasSpeed1 = true;
        
        // 提取第二个数字（电机2速度）
        String speed2Str = input.substring(semicolonIndex + 1);
        speed2 = speed2Str.toInt();
        hasSpeed2 = true;
        
        Serial.print("Parsed speeds: ");
        Serial.print(speed1);
        Serial.print(" and ");
        Serial.println(speed2);
      } else {
        Serial.println("Error: No semicolon found in input!");
      }
    }
    
    // 如果成功读取两个速度值，则控制电机
    if (hasSpeed1 && hasSpeed2) {
      Serial.print("Setting motor 1 to speed: ");
      Serial.println(speed1);
      controlMotor(1, speed1);
      
      Serial.print("Setting motor 2 to speed: ");
      Serial.println(speed2);
      controlMotor(2, speed2);
      
      // 打印当前速度
      Serial.print("Motor 1 Speed: ");
      Serial.print(speed1);
      Serial.print(" | Motor 2 Speed: ");
      Serial.println(speed2);
    } else {
      Serial.println("Invalid input format! Use: speed1;speed2");
      Serial.println("Example: 100;200");
    }
    
    // 清除串口缓冲区
    while (Serial.available() > 0) {
      Serial.read();
    }
  }
}

// 控制指定电机（motorId=1或2）的函数
void controlMotor(int motorId, int speed) {
  // 限制速度范围在-255到255之间
  speed = constrain(speed, -255, 255);
  
  // 根据电机ID选择对应的PWM通道
  int pwmChannelR, pwmChannelL;
  const char* motorName;
  
  if (motorId == 1) {
    pwmChannelR = pwmChannelR1;
    pwmChannelL = pwmChannelL1;
    motorName = "Motor 1";
  } else {
    pwmChannelR = pwmChannelR2;
    pwmChannelL = pwmChannelL2;
    motorName = "Motor 2";
  }
  
  // 根据速度控制电机
  if (speed > 0) {
    // 正转
    Serial.print(motorName);
    Serial.print(" forward at speed: ");
    Serial.println(speed);
    ledcWrite(pwmChannelR, speed);
    ledcWrite(pwmChannelL, 0);
  } else if (speed < 0) {
    // 反转
    Serial.print(motorName);
    Serial.print(" reverse at speed: ");
    Serial.println(abs(speed));
    ledcWrite(pwmChannelR, 0);
    ledcWrite(pwmChannelL, abs(speed));
  } else {
    // 停止
    Serial.print(motorName);
    Serial.println(" stopped");
    ledcWrite(pwmChannelR, 0);
    ledcWrite(pwmChannelL, 0);
  }
}